package Utils;
import lejos.nxt.Motor;
import lejos.nxt.UltrasonicSensor;


public class UltrasonicSensorMethod {
	
	
	public static int getRightDistance (UltrasonicSensor mUltrasonicSensor) throws InterruptedException {
		
		Motor.B.rotateTo(90);
		Thread.sleep(100);
		
		int dist = mUltrasonicSensor.getDistance();
		
		Motor.B.rotateTo(0);
		return dist;
	}
	
	public static int getLeftDistance (UltrasonicSensor mUltrasonicSensor) throws InterruptedException {
		Motor.B.rotateTo(-90);
		Thread.sleep(100);
		
		int dist = mUltrasonicSensor.getDistance();
		
		Motor.B.rotateTo(0);
		return dist;
	}
	
	public static int getForwardDistance (UltrasonicSensor mUltrasonicSensor) throws InterruptedException {
		
		Motor.B.rotateTo(0);
		Thread.sleep(100);
		
		return mUltrasonicSensor.getDistance();
	}
	
	public static int getBackwardDistance (UltrasonicSensor mUltrasonicSensor) throws InterruptedException {
		
		Motor.B.rotateTo(180);
		Thread.sleep(100);
		
		int dist = mUltrasonicSensor.getDistance();
		
		Motor.B.rotateTo(0);
		return dist;
	}
	
	public static int getClosestWallAngle (int angleInterval, UltrasonicSensor mUltrasonicSensor) throws InterruptedException {
		
		int minReading = 255;
		int minAngle = 0;
		
		// Rotate ultrasonic sensor to 0 degree
		Motor.B.rotateTo(0);
		Thread.sleep(100);
		
		// Find the closest wall angle
		for(int i = 150; i > -210; i = i - angleInterval) {
			Motor.B.rotateTo(i);
			if(mUltrasonicSensor.getDistance() < minReading) {
				minReading = mUltrasonicSensor.getDistance();
				minAngle = i;
			}
			Thread.sleep(400);
		}
		
		// rotate ultrasonic sensor to minimum distance angle, just for visualization
		Motor.B.rotateTo(minAngle);
		Thread.sleep(200);
		
		// rotate ultrasonic sensor to 0 degrees
		Motor.B.rotateTo(0);
		Thread.sleep(100);
		
		return minAngle;
	}
}
